
#include <iostream>
#include <time.h>
#include <ros/ros.h> 
#include <tf/transform_datatypes.h>
#include <fstream>  //文件流库函数
#include <iomanip>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <kcf_tracker/kcfTrackerRoi.h>

using namespace std;

ofstream outfile_turtlebotPose;   //输出流
ofstream outfile_uav1Pose;   //输出流
ofstream outfile_trackerResult_uav1;   //输出流

geometry_msgs::PoseStamped turtlebotPose;
geometry_msgs::PoseStamped uav1Pose;
kcf_tracker::kcfTrackerRoi trackerResult_uav1;

void uav1Pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg);
void turtlebotPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg);
void trackerResult_uav1_cb(const kcf_tracker::kcfTrackerRoi::ConstPtr& msg);


double pi = 3.1415926;

int main(int argc, char *argv[])
{
    
    //视频保存位置
    string saveFilePath = "/home/nvidia/work/dataset/saveFile/";  

    ros::init(argc, argv, "save_trackerdata_node");
    ros::NodeHandle nh;//创建句柄
        
    ros::Subscriber turtlebotPose_sub;
    ros::Subscriber uav1Pose_sub;
    ros::Subscriber trackerResult_uav1_sub;


    //设置订阅主题 
    uav1Pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/px4_q250/pose", 1, uav1Pose_cb);
    turtlebotPose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/turtlebot1/pose", 1, turtlebotPose_cb);
    trackerResult_uav1_sub = nh.subscribe<kcf_tracker::kcfTrackerRoi>("/kcfTracker/result/trackRoi", 1, trackerResult_uav1_cb);


    ros::Rate loop_rate(50);

    // 获取当前时间，精确到秒
    time_t currentTime = std::time(NULL);
    char chCurrentTime[64];
    std::strftime(chCurrentTime, sizeof(chCurrentTime), "%Y-%m-%d-%H-%M-%S", std::localtime(&currentTime)); //年月日 时分秒
    std::string stCurrentTime = chCurrentTime;// 转为string
    
    // 文件

    outfile_turtlebotPose.open(saveFilePath + "turtlebotPose" + "-"  + stCurrentTime+ ".txt" , ios::trunc);
    outfile_uav1Pose.open(saveFilePath + "uav1Pose" + "-"  + stCurrentTime+ ".txt" , ios::trunc);
    outfile_trackerResult_uav1.open(saveFilePath + "trackerResult_uav1" + "-"  + stCurrentTime+ ".txt" , ios::trunc);

    if(!outfile_turtlebotPose.is_open() || !outfile_uav1Pose.is_open() || !outfile_trackerResult_uav1.is_open())
    {
        cout << "fail to open!" << endl;
        return -1;
    }

    double last_time =ros::Time::now().toSec();
    while(ros::ok()) 
    {
        if(ros::Time::now().toSec() - last_time >=2)
        {
            cout << "sava_data_node is running!" << endl;
            last_time =ros::Time::now().toSec();
        }
            
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

/*通过vrpn接受bebop位置消息*/
void uav1Pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    tf::Quaternion quat;
    tf::quaternionMsgToTF(msg->pose.orientation, quat);
    double roll, pitch, yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
    roll = roll * 180 / pi;
    pitch = pitch * 180 / pi;
    yaw = yaw * 180 / pi;

    double secs =ros::Time::now().toNSec()/1000000000.0;
    outfile_uav1Pose << setiosflags(ios::fixed) << setprecision(7) 
                     << secs << "\t"
                     << msg->pose.position.x << "\t" << msg->pose.position.y << "\t" << msg->pose.position.z << "\t"
                     << roll << "\t" << pitch << "\t" << yaw
                     << endl;
}

void turtlebotPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    tf::Quaternion quat;
    tf::quaternionMsgToTF(msg->pose.orientation, quat);
    double roll, pitch, yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
    roll = roll * 180 / pi;
    pitch = pitch * 180 / pi;
    yaw = yaw * 180 / pi;

    double secs =ros::Time::now().toNSec()/1000000000.0;
    outfile_turtlebotPose << setiosflags(ios::fixed) << setprecision(7) 
                     << secs << "\t"
                     << msg->pose.position.x << "\t" << msg->pose.position.y << "\t" << msg->pose.position.z << "\t"
                     << roll << "\t" << pitch << "\t" << yaw
                     << endl;
}

void trackerResult_uav1_cb(const kcf_tracker::kcfTrackerRoi::ConstPtr& msg)
{    
    double secs =ros::Time::now().toNSec()/1000000000.0;
    // uint16 state = msg->state;
    outfile_trackerResult_uav1 << setiosflags(ios::fixed) << setprecision(7) 
                    << secs << "\t"
                    << msg->x << "\t" << msg->y << "\t" << msg->width << "\t"
                    << msg->height << "\t" << msg->distance << "\t" << (int)msg->state
                    << endl;
}

